#include "sockets.h"
#include "emac_params.h"
#include "sh_mem.h"
#include "emac.h"
#include "os_mem.h"
#include "sw_interrupt.h"
#include "os_io.h"

static bool endianess_found = FALSE;
static bool little_endian = FALSE;

void find_endianess(void)
{
  uint16_t value = 0xABCD;
  uint8_t *ptr = (uint8_t *)&value;

  little_endian = FALSE;
  if (*ptr == 0xCD) { little_endian = TRUE; }
  endianess_found = TRUE;
  return;
}

uint16_t htons(uint16_t value)
{
    if (!endianess_found) {
        find_endianess();
    }

    if (little_endian) {
        return (uint16_t)(((value & 0xFF) << 8) | (value >>8));
    } else {
        return value;
    }
}

uint32_t htonl(uint32_t value)
{
    if (!endianess_found) {
        find_endianess();
    }

    if (little_endian) {
        return ( (((value >>0) & 0xFF) << 24) |  (((value >>8) & 0xff) << 16) |
	         (((value >>16) & 0xFF) <<8) | (((value >>24) & 0xff) << 0));
    } else {
        return value;
    }
}

static inline
err_t open_network_socket(uint8_t *name, void **handle, uint8_t sub_type, uint16_t port_no)
{
    queue_handle_t queue_handle = NULL;
    socket_t *sock = NULL;
    emac_params_t *emac_params = NULL;
    void *shm_handle = NULL;
    err_t err_code;

    if (!port_no) {
        return ERR_INVALID;
    }

    /*
    * Get handle to shared memory containing emac parameters
    * for transmitting the packet out
    */
    err_code = open_shared_memory(EMAC_PARAMS_MEM, &shm_handle, (void **)&emac_params);
    if (err_code != ERR_SUCCESS) {
       if (err_code == ERR_NOT_FOUND) { err_code = ERR_AGAIN; }
       printf("Could not open shared memory %s. err:0x%x\n", EMAC_PARAMS_MEM, err_code);
       return err_code;
    }

    // Create a queue for the socket
    err_code = create_queue(name, NULL, &queue_handle);
    if (err_code != ERR_SUCCESS) {
        printf("Could not open queue. err:0x%x\n", err_code);
        close_shared_memory(shm_handle);
        return err_code;
    }

    sock = (socket_t *)calloc(1, sizeof(socket_t));
    if (!sock) {
        printf("Socket allocation failed.\n");
        close_shared_memory(shm_handle);
	destroy_queue(queue_handle);
	return ERR_NO_MEM;
    }

    sock->emac_params = emac_params;
    sock->shmem_handle = shm_handle;
    sock->queue_handle = queue_handle;

    sock->dev_handle.dev_info.name = NULL;
    sock->dev_handle.dev_info.type = DEVICE_TYPE_NETWORK_SOCKET;
    sock->dev_handle.dev_info.subtype = sub_type;
    sock->dev_handle.dev_info.address = port_no;

    sock->dev_handle.open_mode = DEVICE_MODE_CREATE_ALWAYS;
    sock->dev_handle.access_info.permissions = DEVICE_MODE_WRITE;
    sock->dev_handle.qid = queue_handle->queue_id;

    //printf("Set type:0x%x in pointer: 0x%x\n", sock->dev_handle.dev_info.type, &sock->dev_handle);
    raise_sw_interrupt(SW_INT_DEVICE_OPEN, (uint32_t)&sock->dev_handle);

    err_code = sock->dev_handle.err_code;
    if (err_code != ERR_SUCCESS) {
        free(sock);
	close_shared_memory(shm_handle);
	destroy_queue(queue_handle);
	*handle = NULL;
    } else {
        *handle = (void *)sock;
    }

    return err_code;
}

err_t socket_timer_start(void *handle, user_timer_data_t *timer_data)
{
    socket_t *socket = (socket_t *)handle;

    if (!socket || !timer_data || !socket->queue_handle) {
        return ERR_INVALID;
    }

    timer_data->q_id = socket->queue_handle->queue_id;
    return user_timer_start(timer_data);
}


err_t set_sock_opt(void *handle, sock_opt_type sock_opt, uint32_t value)
{
    socket_t *sock = (socket_t *)handle;

    if (!handle || sock_opt >= SOCK_OPT_MAX) {
        return ERR_INVALID;
    }

    switch(sock_opt) {
        case SOCK_OPT_IPC_HANDLER:
	    sock->ipc_msg_handler = (void *)value;
	    break;

	default:
	    printf("Invalid socket option: 0x%x\n", sock_opt);
	    return ERR_INVALID;
    }

    return ERR_SUCCESS;
}
      

err_t rcv_socket(void *handle, socket_data_t *sock_data, uint8_t *buffer, uint16_t *len)
{
    socket_t *socket = (socket_t *)handle;
    err_t err_code;
    queue_msg_t msg;
    uint8_t offset = 0;
    uint16_t msg_len = 0;

    if (!handle || !buffer || !len || !socket->queue_handle || 
        !sock_data || *len == 0) {
        return ERR_INVALID;
    }

    while(1) {
        err_code = dequeue(socket->queue_handle, &msg);
        if (err_code != ERR_SUCCESS) {
            return err_code;
        }
    
        if (msg.msg_id == Q_MSG_TYPE_IPC) {
            // If client has regisered an IPC handler, then submit 
            // the message to it
            if (socket->ipc_msg_handler) {
                socket->ipc_msg_handler(msg.msg_data, msg.msg_len);
            } else {
                free(msg.msg_data);
            }
        
            continue;
	}

	break;
    }

    sock_data->ip_addr = ntohl(((ip_hdr_t *)msg.msg_data)->source_ip);

    if (socket->dev_handle.dev_info.subtype == DEVICE_SUBTYPE_IP_SOCKET) {
        offset = OFFSET_OF(data, ip_hdr_t);
	sock_data->port = (uint16_t)socket->dev_handle.dev_info.address;
    } else {
        offset = OFFSET_OF(data, udp_hdr_t);
	sock_data->port = ntohs(((udp_hdr_t *)msg.msg_data)->src_port);
    }

    msg_len = (*len) >= (msg.msg_len - offset) ? (msg.msg_len - offset) : (*len);

    memcpy(buffer, ((uint8_t *)msg.msg_data + offset), msg_len);
    *len = msg_len;

    free(msg.msg_data);

    return err_code;
}

uint16_t calc_ip_checksum(uint16_t *data, uint8_t words)
{
    uint8_t i = 0;
    uint32_t csum = 0;

    for (i = 0; i < words; i++) {
        csum += *data;
	if (csum > 0xFFFF) {
	    csum++;
	    csum &= 0xFFFF;
	}
    }

    return (uint16_t)csum;
}
        
static inline
err_t fill_ip_hdr(ip_hdr_t *header, socket_t *sock, uint32_t dst_ip, uint16_t ip_port, uint16_t payload_len)
{
  err_t err_code = ERR_SUCCESS;
  uint16_t i = 0;

  uint32_t csum = 0;
  uint16_t *csum_ptr = NULL;

  // Dst mac shall be filled by driver
  for (i = 0; i < 6; i++) {
     header->dst_mac[i] = 0x00;  // 00 means mac to be filled by driver
  }

  memcpy(header->src_mac, sock->emac_params->mac_addr, 6);
  header->l3_protocol = htons(0x0800);  // IP
  header->hlen = 5;
  header->version = 4;
  header->extra = 0;
  header->total_len = htons(20 + payload_len);
  header->ident = 0;
  header->frag_flags = 0;
  header->ttl = 0xFF;
  header->l4_proto = ip_port;
  header->hdr_csum = 0;
  header->source_ip = htonl(sock->emac_params->ip_addr);
  header->dest_ip = htonl(dst_ip);

  csum_ptr = (uint16_t *)&header->l3_protocol + 1;
  for (i = 0; i < 10; i++) {
      csum += *csum_ptr;
      if (csum > 0xFFFF) {  // carry
          csum++; // add carry bit
	  csum = csum & 0xFFFF;
      }
      csum_ptr++;
  }

  header->hdr_csum = ~csum;

  return err_code;
}

static inline
err_t fill_udp_hdr(udp_hdr_t *header, socket_t *sock, uint32_t dst_ip, uint16_t udp_port, uint16_t payload_len)
{
    err_t err_code = ERR_SUCCESS;

    err_code = fill_ip_hdr((ip_hdr_t *)header, sock, dst_ip, 0x11, (payload_len + 8));
    if (err_code != ERR_SUCCESS) {
        return err_code;
    }

    header->src_port = htons(sock->dev_handle.dev_info.address);
    header->dst_port = htons(udp_port);
    header->udp_len = htons(payload_len);
    header->checksum = htons(0);

    return err_code;
}


err_t send_on_socket(void *sock_handle, socket_data_t *sock_data)
{
    uint8_t *header = NULL;
    uint16_t header_size = 0;
    socket_t *sock = (socket_t *)sock_handle;
    err_t err_code = ERR_SUCCESS;
    gather_list_t *gather_list = NULL;

    if (!sock || !sock_data || !sock->emac_params) {
        return ERR_INVALID;
    }

    if (!sock_data->ip_addr || !sock_data->port || !sock_data->data || !sock_data->len) {
        return ERR_INVALID;
    }

    gather_list = (gather_list_t *)calloc(1, sizeof(gather_list_t));
    if (!gather_list) {
        return ERR_NO_MEM;
    }

    if (sock->dev_handle.dev_info.subtype == DEVICE_SUBTYPE_UDP_SOCKET) {
        header = calloc(1, sizeof(udp_hdr_t));
	header_size = sizeof(udp_hdr_t);
	if (!header) { 
	    free(gather_list); 
	    return ERR_NO_MEM;
	}
	err_code = fill_udp_hdr((udp_hdr_t *)header, sock, sock_data->ip_addr, sock_data->port, sock_data->len);
    } else {
        // Raw IP socket
	header = calloc(1, sizeof(ip_hdr_t));
	header_size = sizeof(ip_hdr_t);
	if (!header) { 
	    free(gather_list);
	    return ERR_NO_MEM;
	}
	err_code = fill_ip_hdr((ip_hdr_t *)header, sock, sock_data->ip_addr, sock_data->port, sock_data->len);
    }

    if (err_code != ERR_SUCCESS) {
        free(header);
	free(gather_list);
	return err_code;
    }

    gather_list->num_entries = 2;
    gather_list->buf_list[0].buf_ptr = (uint8_t *)header;
    gather_list->buf_list[0].len = header_size;
    gather_list->buf_list[1].buf_ptr = sock_data->data;
    gather_list->buf_list[1].len = sock_data->len;
    gather_list->next = NULL;

    //printf("Socket library enqueueing on 0x%x\n", sock->emac_params->tx_queue_id);
    err_code = enqueue_shared_data(sock->emac_params->tx_queue_id, (uint8_t *)gather_list, sizeof(gather_list_t));
    if (err_code != ERR_SUCCESS) {
        free(gather_list);
	free(header);
    }

    return err_code;
}


err_t socket_close(void *handle)
{
    socket_t *sock = (socket_t *)handle;

    if (!sock) {
        return ERR_INVALID;
    }

    close_shared_memory(sock->shmem_handle);
    destroy_queue(sock->queue_handle);
    free(sock);

    return ERR_SUCCESS;
}

err_t open_udp_socket(uint8_t *name, uint16_t port_no, void **sock_handle)
{
    return open_network_socket(name, sock_handle, DEVICE_SUBTYPE_UDP_SOCKET, port_no);
}

err_t open_ip_socket(uint8_t *name, uint16_t port_no, void **sock_handle)
{
    return open_network_socket(name, sock_handle, DEVICE_SUBTYPE_IP_SOCKET, port_no);
}

